#!/usr/bin/env python

# Copyright 2012 Colin McDonough, Troy Velius

import roslib; roslib.load_manifest('pr2_hri')
import rospy
from std_msgs.msg import String
import actionlib
import arm_navigation_msgs.msg
import math
import pr2_hri.msg
from tf.transformations import quaternion_from_euler

RAISE_CAMERA_TO_EYE = 'raise_camera_to_eye'
LOWER_CAMERA_FROM_EYE = 'lower_camera_from_eye'

def poseConstraintToPositionOrientationConstraints(pose_constraint):
  position_constraint = arm_navigation_msgs.msg.PositionConstraint()
  position_constraint.header = pose_constraint.header
  position_constraint.link_name = pose_constraint.link_name
  position_constraint.position = pose_constraint.pose.position
  position_constraint.constraint_region_shape.type = arm_navigation_msgs.msg.Shape.BOX
  position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x)
  position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y)
  position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z)

  position_constraint.constraint_region_orientation.x = 0.0
  position_constraint.constraint_region_orientation.y = 0.0
  position_constraint.constraint_region_orientation.z = 0.0
  position_constraint.constraint_region_orientation.w = 1.0

  position_constraint.weight = 1.0

  orientation_constraint=arm_navigation_msgs.msg.OrientationConstraint()
  orientation_constraint.header = pose_constraint.header
  orientation_constraint.link_name = pose_constraint.link_name
  orientation_constraint.orientation = pose_constraint.pose.orientation
  orientation_constraint.type = pose_constraint.orientation_constraint_type

  orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
  orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
  orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
  orientation_constraint.weight = 1.0

  return position_constraint, orientation_constraint

def addGoalConstraintToMoveArmGoal(desired_pose, goalA):
  position_constraint, orientation_constraint= poseConstraintToPositionOrientationConstraints(desired_pose);
  goalA.motion_plan_request.goal_constraints.position_constraints.append(position_constraint);
  goalA.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint);

class HriActionServer(object):
  # create messages that are used to publish feedback/result
  _feedback = pr2_hri.msg.hriFeedback()
  _result   = pr2_hri.msg.hriResult()

  def __init__(self, name):
    self._action_name = name
    self.client = actionlib.SimpleActionClient('move_right_arm', arm_navigation_msgs.msg.MoveArmAction)
    rospy.loginfo('before wait_for_server')
    self.client.wait_for_server()
    rospy.loginfo('after wait_for_server')
    self._as = actionlib.SimpleActionServer(self._action_name, pr2_hri.msg.hriAction, execute_cb=self.execute_cb, auto_start=False)
    self._as.start()

  def execute_cb(self, goal):
    rospy.loginfo('Recieved goal: %s' % goal.goalString.data)

    if goal.goalString.data == RAISE_CAMERA_TO_EYE:

      goal_a = arm_navigation_msgs.msg.MoveArmGoal()
      goal_a.motion_plan_request.group_name = 'right_arm'
      goal_a.motion_plan_request.num_planning_attempts = 10
      goal_a.motion_plan_request.planner_id = ''
      goal_a.planner_service_name = 'ompl_planning/plan_kinematic_path'
      goal_a.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)

      desired_pose = arm_navigation_msgs.msg.SimplePoseConstraint()
      desired_pose.header.frame_id = '/torso_lift_link'
      desired_pose.link_name = 'r_wrist_roll_link'
      desired_pose.pose.position.x = 0.45
      desired_pose.pose.position.y = -0.188
      desired_pose.pose.position.z = 0.45

      (x, y, z, w) = quaternion_from_euler(math.pi / 2.0, 0.0, math.pi / 2.0)
      desired_pose.pose.orientation.x = x
      desired_pose.pose.orientation.y = y
      desired_pose.pose.orientation.z = z
      desired_pose.pose.orientation.w = w

      desired_pose.absolute_position_tolerance.x = 0.02
      desired_pose.absolute_position_tolerance.y = 0.02
      desired_pose.absolute_position_tolerance.z = 0.02

      desired_pose.absolute_roll_tolerance = 0.04
      desired_pose.absolute_pitch_tolerance = 0.04
      desired_pose.absolute_yaw_tolerance = 0.04

      addGoalConstraintToMoveArmGoal(desired_pose, goal_a)

      finished_within_time = False
      self.client.send_goal(goal_a)
      rospy.loginfo('Goal Sent')
      finished_within_time = self.client.wait_for_result(rospy.Duration(200.0))
      if not finished_within_time:
        # Timed out achieving goal
        self.client.cancel_goal()
      else:
        state = self.client.get_state()
        rospy.loginfo('State %s' % state)

      self._feedback.feedbackString = String('feedback')
      self._as.publish_feedback(self._feedback)

      self._result.resultString= String('succeeded')
      self._as.set_succeeded(self._result)
    elif goal.goalString.data == LOWER_CAMERA_FROM_EYE:

      goal_a = arm_navigation_msgs.msg.MoveArmGoal()
      goal_a.motion_plan_request.group_name = 'right_arm'
      goal_a.motion_plan_request.num_planning_attempts = 10
      goal_a.motion_plan_request.planner_id = ''
      goal_a.planner_service_name = 'ompl_planning/plan_kinematic_path'
      goal_a.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)

      desired_pose = arm_navigation_msgs.msg.SimplePoseConstraint()
      desired_pose.header.frame_id = '/torso_lift_link'
      desired_pose.link_name = 'r_wrist_roll_link'
      desired_pose.pose.position.x = 0.75
      desired_pose.pose.position.y = -0.188
      desired_pose.pose.position.z = 0.0

      (x, y, z, w) = quaternion_from_euler(math.pi / 2.0, 0.0, math.pi / 2.0)
      desired_pose.pose.orientation.x = x
      desired_pose.pose.orientation.y = y
      desired_pose.pose.orientation.z = z
      desired_pose.pose.orientation.w = w

      desired_pose.absolute_position_tolerance.x = 0.02
      desired_pose.absolute_position_tolerance.y = 0.02
      desired_pose.absolute_position_tolerance.z = 0.02

      desired_pose.absolute_roll_tolerance = 0.04
      desired_pose.absolute_pitch_tolerance = 0.04
      desired_pose.absolute_yaw_tolerance = 0.04

      addGoalConstraintToMoveArmGoal(desired_pose, goal_a)

      finished_within_time = False
      self.client.send_goal(goal_a)
      rospy.loginfo('Goal Sent')
      finished_within_time = self.client.wait_for_result(rospy.Duration(200.0))
      if not finished_within_time:
        # Timed out achieving goal
        self.client.cancel_goal()
      else:
        state = self.client.get_state()
        rospy.loginfo('State %s' % state)

      self._feedback.feedbackString = String('feedback')
      self._as.publish_feedback(self._feedback)

      self._result.resultString= String('succeeded')
      self._as.set_succeeded(self._result)

    else:
      rospy.loginfo('Goal %s not recognized' % goal.goalString.data)
      self._result.resultString = String('aborted')
      self._as.set_aborted(self._result)

if __name__ == '__main__': 
  rospy.init_node('hriActionServer', log_level=rospy.DEBUG)
  rospy.loginfo('Server_Main')
  HriActionServer(rospy.get_name())
  rospy.spin()

